import rclpy
from rclpy.node import Node
from std_msgs.msg import UInt64, Float64
import random
import time
import math
import struct

class CanDeviceSimulator(Node):
    def __init__(self):
        super().__init__('can_device_simulator')
        
        # 设备参数配置
        self.declare_parameters(
            namespace='',
            parameters=[
                ('earth_node_id', 8),     # 地面相机节点ID
                ('shelf_node_id', 9),     # 货架相机节点ID
                ('publish_freq', 50.0),   # 发布频率
                ('enable_timestamp', True) # 是否启用时间戳
            ]
        )
        
        self.earth_node_id = self.get_parameter('earth_node_id').value
        self.shelf_node_id = self.get_parameter('shelf_node_id').value
        self.publish_freq = self.get_parameter('publish_freq').value
        self.enable_timestamp = self.get_parameter('enable_timestamp').value
        
        self.pdo_publishers = {}
        self.current_tag = 1000  # 初始二维码标签
        self.sequence = 0        # 数据序列号
        
        self.init_publishers(self.earth_node_id)
        self.init_publishers(self.shelf_node_id)
        
        self.timer = self.create_timer(1.0/self.publish_freq, self.timer_callback)
        self.get_logger().info(f"CAN设备模拟器已启动，频率: {self.publish_freq}Hz")

    def init_publishers(self, node_id):
        base_topic = f"/can0/device{node_id}/get_pdo_raw_data_"
        
        # 创建4个PDO数据发布器
        for pdo_num in range(4):
            topic_name = base_topic + str(pdo_num)
            self.pdo_publishers[(node_id, pdo_num)] = self.create_publisher(
                UInt64, topic_name, 10)
            
        # 创建时间戳发布器（如果启用）
        if self.enable_timestamp:
            ts_topic = base_topic + "2/timestamp"
            self.pdo_publishers[(node_id, 'ts')] = self.create_publisher(
                Float64, ts_topic, 10)

    def timer_callback(self):
        # 生成测试数据并发布
        for node_id in [self.earth_node_id, self.shelf_node_id]:
            self.generate_realistic_data(node_id)
            self.publish_data(node_id)
        
        self.sequence += 1

    def generate_realistic_data(self, node_id):
        """生成符合实际业务逻辑的测试数据"""
        # 生成有效范围内的数据（单位：0.0001米）
        xpos = random.randint(-500, 500)  # ±0.05米范围
        ypos = random.randint(-500, 500)
        angle = random.randint(-900, 900) # ±90度范围
        
        # 每10秒更换二维码标签
        if self.sequence % (self.publish_freq * 10) == 0:
            self.current_tag += 1
        
        # PDO数据打包（按C++解析顺序）
        self.pdo_data = {
            0: self.pack_uint32(ypos),      # PDO0: Y位置
            1: self.pack_uint32(xpos) | (self.pack_uint16(angle) << 32), # PDO1: X位置+角度
            2: self.pack_uint64(self.pack_uint32(self.current_tag) << 32), # PDO2: 标签
            3: self.pack_uint32(self.sequence)     # PDO3: 序列号/时间戳
        }
        
        # 时间戳数据（精确到毫秒）
        self.timestamp = time.time()

    def pack_uint16(self, value):
        return value & 0xFFFF

    def pack_uint32(self, value):
        return value & 0xFFFFFFFF

    def pack_uint64(self, value):
        return value & 0xFFFFFFFFFFFFFFFF

    def publish_data(self, node_id):
        """发布指定节点的数据"""
        try:
            # 发布PDO数据
            for pdo_num in range(4):
                msg = UInt64()
                msg.data = self.pdo_data[pdo_num]
                self.pdo_publishers[(node_id, pdo_num)].publish(msg)
            
            # 发布时间戳
            if self.enable_timestamp:
                ts_msg = Float64()
                ts_msg.data = self.timestamp
                self.pdo_publishers[(node_id, 'ts')].publish(ts_msg)
                
        except KeyError as e:
            self.get_logger().error(f"Publisher not found for {node_id}: {str(e)}")

    def destroy_node(self):
        self.get_logger().info("正在关闭模拟器...")
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    
    try:
        simulator = CanDeviceSimulator()
        rclpy.spin(simulator)
    except KeyboardInterrupt:
        pass
    finally:
        simulator.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()